#include "cv.h"
#include <highgui.h>
#include "opencv2/imgproc/imgproc.hpp"
#include "cv_bridge/cv_bridge.h"

#include <image_transport/image_transport.h>

#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Int8MultiArray.h>

#include <string.h>
#include <signal.h>
#include <cmath>

using namespace cv;
using namespace roboard_drivers;
using namespace gr8;

ros::Publisher detection_pub;
image_transport::Publisher imageCam0_pub;
image_transport::Publisher imageCam1_pub;

const double PI = 3.141592653589793238462;

int tagDetector(IplImage *img, int cam);
int findSquare(IplImage* img, int cam);
Mat RedDetectorHSV(IplImage *img);
Mat RedDetectorRGB(IplImage *img);

std_msgs::Int32 msg;		

CvRect imageROI;

/*
This node is doing :
	-> Receive images from camera 0 and 1
	-> For each image, detect if there is a tag using red detector and send a message to detection_pub
	-> If there is a tag 
		1. Look for a square in the image
		2. If the tag is ok, send the original image to imageCam0_pub or imageCam1_pub
*/

cv_bridge::CvImage cvi;

void cam0_cb(const std_msgs::Int8MultiArray::ConstPtr& array)
{
	// Receive the image
	IplImage *img 				= cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
	char * data 				= img->imageData;
	
	for(int i = 0; i < 320*240*3; i++)
	{
		data[i] = char(array->data.at(i));
	}

	// Use the red detector to look for a tag
	int tagDetect = tagDetector(img,0);

	if (tagDetect == 1) {
		// Publish a message to say that ze found a tag
		printf("Tag detected Cam0 \n");
		msg.data = 0;
		detection_pub.publish(msg);
		
		// Look for a square in the image
		int square = findSquare(img, 0);
		
		// if square != 1 then we dont have the complete tag on the imag
		if (square)
		{
			// set the Region of Interest
			cvSetImageROI(img, imageROI); 

			// create destination image
			IplImage *tagImg = cvCreateImage(cvGetSize(img),img->depth,img->nChannels);
			cvCopy(img, tagImg, NULL);	 
			cvResetImageROI(img);

			// convert OpenCV image to ROS message and publish it
			cvi.header.frame_id = "image";
			cvi.encoding = "bgr8";
			cvi.image = tagImg;
			sensor_msgs::Image im;
		    	cvi.toImageMsg(im);

			imageCam0_pub.publish(im);
			printf("Tag sent \n");
			cvReleaseImage(&tagImg);
		}
	}
    cvReleaseImage(&img);
}

void cam1_cb(const std_msgs::Int8MultiArray::ConstPtr& array)
{
	// Receive the image
	IplImage *img 				= cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
	char * data 				= img->imageData;
	
	for(int i = 0; i < 320*240*3; i++)
	{
		data[i] = char(array->data.at(i));
	}

	// Use the red detector to look for a tag
	int tagDetect = tagDetector(img,1);

	if (tagDetect == 1) {
		// Publish a message to say that ze found a tag
		printf("Tag detected Cam1 \n");
		msg.data = 0;
		detection_pub.publish(msg);
		
		// Look for a square in the image
		int square = findSquare(img, 1);
		
		// if square != 1 then we dont have the complete tag on the imag
		if (square)
		{
			// set the Region of Interest
			cvSetImageROI(img, imageROI); 

			// create destination image
			IplImage *tagImg = cvCreateImage(cvGetSize(img),img->depth,img->nChannels);
			cvCopy(img, tagImg, NULL);	 
			cvResetImageROI(img);

			// convert OpenCV image to ROS message and publish it
			cvi.header.frame_id = "image";
			cvi.encoding = "bgr8";
			cvi.image = tagImg;
			sensor_msgs::Image im;
		    	cvi.toImageMsg(im);

			imageCam1_pub.publish(im);
			printf("Tag sent \n");
			cvReleaseImage(&tagImg);
		}
	}
    cvReleaseImage(&img);
}

/* This function is looking for a tag using the two red detectors defined below */
int tagDetector(IplImage *img, int cam)
{
	// Create an image for the output
	Mat out(cvGetSize(img), CV_8UC1);

	// Use the two red detectors
	Mat RedHSV = RedDetectorHSV(img);
	Mat RedRGB = RedDetectorRGB(img);

	// Combine the results
	bitwise_and(RedHSV, RedRGB, out);

	// Use a threshold on the number of white pixels
	int n = countNonZero(out);
	if (n>500) 
	{
		return 1;
	} else 
	{
		return 0;
	}
	
}

/* This function is looking for red in the image using RGB value */
Mat RedDetectorRGB(IplImage *img) 
{
	double RThresholdLow = 80; 
	double GThresholdHigh = 80;
	double BThresholdHigh = 80;
	int maxValue = 255;

	// Separate color channels
	//Order is BGR
	IplImage* blue = cvCreateImage( cvGetSize(img), IPL_DEPTH_8U, 1 );
	IplImage* green = cvCreateImage( cvGetSize(img), IPL_DEPTH_8U, 1 );
	IplImage* red = cvCreateImage( cvGetSize(img), IPL_DEPTH_8U, 1 );
	cvSplit(img, blue, green, red, NULL);

	Mat RedMask(red);
	Mat GreenMask(green);
	Mat BlueMask(blue);

	// Thresholding
	threshold(RedMask, RedMask, RThresholdLow, maxValue, THRESH_BINARY);
	threshold(GreenMask, GreenMask, GThresholdHigh, maxValue, THRESH_BINARY_INV);
	threshold(BlueMask, BlueMask, BThresholdHigh, maxValue, THRESH_BINARY_INV);

	Mat out(cvGetSize(img), CV_8UC1);
	// Combine the masks
	bitwise_and(RedMask, GreenMask, out);
	bitwise_and(out, BlueMask, out);

	cvReleaseImage(&blue);
	cvReleaseImage(&green);
	cvReleaseImage(&red);
	return out;
}

/* This function is looking for red in the image using HSV value */
Mat RedDetectorHSV(IplImage *img) 
{

	double hueThresholdLow = 0.3; 
	int maxValue = 255;

	// Separate color channels
	// Convert into HSV
	Mat imgMask(img);
	Mat imgHSV(cvGetSize(img), CV_8UC1);
	cvtColor(imgMask, imgHSV, CV_BGR2HSV, 0);

	//Split the color channels
	vector<Mat> channels(3);
	split(imgHSV, channels);
	Mat HueMask = channels[1];
	Mat SatMask = channels[0];
	Mat ValMask = channels[2];

	// Thresholding
	threshold(HueMask, HueMask, hueThresholdLow, maxValue, THRESH_BINARY);
	threshold(SatMask, SatMask, 128, maxValue, THRESH_BINARY | THRESH_OTSU); 

	Mat out(cvGetSize(img), CV_8UC1);
	// Combine the masks
	bitwise_and(HueMask, SatMask, out);

	return out;
}

// Function to find a square in the image (to verify if the tag is complete and grab it)
int findSquare(IplImage* img, int cam)
{

	// Convert image to greyscale then to binary
	IplImage* greyImg = cvCreateImage( cvGetSize(img), IPL_DEPTH_8U, 1 );
	cvCvtColor(img, greyImg, CV_BGR2GRAY); 
	Mat imgGreyMask(greyImg);
	Mat binary(cvGetSize(img), CV_8UC1);
	threshold(imgGreyMask, binary, 80, 255, THRESH_BINARY_INV);
	
	// Apply Canny on the image
	Mat imgToProcess;
	Canny(binary, imgToProcess, 70,180, 3);

	// Find contours in the image
	Mat out;
	imgToProcess.copyTo(out);
	vector<vector<Point> > contours;
	findContours( out, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );

	for( int i = 0; i< contours.size(); i++ )
	{
		vector<Point> contouri = contours.at(i);
		vector<Point> contours_poly;
		
		// Approximate the contour with a polygon
		approxPolyDP( Mat(contouri), contours_poly, 2, true );

		// A square must have 4 vertices, be convex and have a large area (to remove noise)
		double area = contourArea(contours_poly);
		bool convex = isContourConvex(contours_poly);
		if ((contours_poly.size() == 4) && convex && area>1000)
		{
			printf("Square detected\n");
			imageROI = boundingRect( Mat(contours_poly) );
			return 1;
		}
	}
	return 0;

}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "RedDetector");
	ros::NodeHandle n;
	ros::Subscriber img0_sub = n.subscribe("/camera0_img", 1, cam0_cb);
	ros::Subscriber img1_sub = n.subscribe("/camera1_img", 1, cam1_cb);

	detection_pub = n.advertise<std_msgs::Int32>("detection_pub", 1);

	image_transport::ImageTransport it(n);
	imageCam0_pub = it.advertise("camera/imageCam0", 1);
	imageCam1_pub = it.advertise("camera/imageCam1", 1);
	ros::spin();
}




